package CamposDePotencial;

import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.IOException;
import java.io.InputStream;
import java.util.Properties;

import javax.vecmath.Point3d;

import estrategia.ParametrosPotenciales;
import estrategia.Predictor;
import estrategia.Utiles;
import estrategia.Vector;

public class CampoTangencialRepulsor extends CampoPotencial{

	private double radioCampo;
	private double minAlcance;
	private double Krepulsion;
	private double tamanioRobot;
	
	private double direccionObjetivo, direccionPerpendicularObjetivo, mObjetivo, mPerpendicular;
	private Objetivo objetivo;
	
	public CampoTangencialRepulsor(int idObjeto, Objetivo objetivo, Utiles u, Predictor predictor) {
		// TODO Auto-generated constructor stub
		super(idObjeto, u, predictor);
		this.objetivo = objetivo;
	}
	
	
	public void  cargarConstantes() {
		radioCampo = ParametrosPotenciales.radioCampoTangencial;
		minAlcance = ParametrosPotenciales.minAlcanceTangencial;
		Krepulsion = ParametrosPotenciales.KCampoTangencial;
		tamanioRobot = ParametrosPotenciales.tamanioRobot;	
	}
	
	public double hallarPendienteRecta(Point3d posObjeto, Point3d posObjetivo) {
		return (posObjetivo.y - posObjeto.y)/(posObjetivo.x - posObjeto.x);
	}
	
	
	public double evaluarRecta(double m, Point3d posRobot) {			
		return (m*posRobot.x - posRobot.y);
	}
	
	
	public double evaluarCircunferencia(Point3d posObjeto, Point3d posRobot) {
		double valor = Math.pow((posRobot.x - posObjeto.x), 2) + Math.pow((posRobot.y - posObjeto.y), 2); 
		return Math.sqrt(valor); 
	}
	
	public double recta(Point3d posObj, double m, Point3d mipos) {
		double eval = 0;
		if (m == Double.NEGATIVE_INFINITY)
			eval = mipos.x - posObj.x;
		else if (m == Double.POSITIVE_INFINITY)
				eval = -(mipos.x - posObj.x);
		else 
			eval = ((mipos.y - posObj.y) - m*(mipos.x - posObj.x));
		//System.out.println("Evalue la recta en " + eval);
		return eval;
	}
		
	
	
	public double calcularDireccion(Point3d mipos) throws IOException {
		double direccion = 0;
		
		Point3d posObj = predictor.predecirPosicion(idObjeto);
		direccionObjetivo = utiles.getAnguloTheta(posObj, objetivo.getPosicionObjetivo());		
		Point3d PObjetivo = objetivo.getPosicionObjetivo();
		mObjetivo = hallarPendienteRecta(posObj, PObjetivo);
		
		double anguloRobot = utiles.getAnguloTheta(mipos, posObj);
		
		//System.out.println("La direccion de mi robot mirado desde el objetivo es: " + utiles.getAnguloTheta(posObj, mipos));
		
		//if (direccionObjetivo <= (Math.PI/2.0) && direccionObjetivo >= -(Math.PI/2.0)) {
		if ((mObjetivo >= 0 && direccionObjetivo >= 0) || (mObjetivo <= 0 && direccionObjetivo <= 0)) {
			//System.out.println("dir Objetivo = " + direccionObjetivo + " y mObjetivo es = " + mObjetivo);
			//if (Math.signum(evaluarRecta(mObjetivo, mipos) - (mObjetivo*posObj.x -posObj.y ))  >= 0) {
			if (recta(posObj, mObjetivo, mipos) >= 0 ) {
				// Tengo que sumar 90
				//System.out.println("1. Sumar 90 y mObjetivo = " + mObjetivo + " direccionObjetivo = " + direccionObjetivo);
				direccion = anguloRobot + Math.PI/2;
				if (direccion > Math.PI)
					direccion = -Math.PI + (direccion - Math.PI);
			} else {
				//System.out.println("1. Restar 90 y mObjetivo = " + mObjetivo + " direccionObjetivo = " + direccionObjetivo);
				// Tengo que restar 90
				direccion = anguloRobot - Math.PI/2;
				if (direccion < -Math.PI)
					direccion = Math.PI + (direccion + Math.PI);
			}
		} else {
			//if (Math.signum(evaluarRecta(mObjetivo, mipos)) - (mObjetivo*posObj.x -posObj.y ) >= 0) {
			if (recta(posObj, mObjetivo, mipos) >= 0) {
				//System.out.println("2. Restar 90 y mObjetivo = " + mObjetivo + " direccionObjetivo = " + direccionObjetivo);
				// Tengo que restar 90
				direccion = anguloRobot - Math.PI/2;
				if (direccion < -Math.PI)
					direccion = Math.PI + (direccion + Math.PI);
			} else {
				//System.out.println("2. Sumar 90 y mObjetivo = " + mObjetivo + " direccionObjetivo = " + direccionObjetivo);
				// Tengo que sumar 90
				direccion = anguloRobot + Math.PI/2;
				if (direccion > Math.PI)
					direccion = -Math.PI + (direccion - Math.PI);
				
			}
		}
			
		
		return direccion;
	}
	public double getMagnitudFuerza(Point3d mipos, Point3d posObj) {
		double magnitud = 0;
		double distancia = utiles.distanciaEuclidea(mipos, posObj);
		distancia = distancia - tamanioRobot;		
		
		if (evaluarCircunferencia(posObj, mipos) <= radioCampo) { 
				magnitud = Krepulsion*((1/distancia) - (1/radioCampo));
		}
		
		
		return magnitud;
	}
	
	
	public Vector obtenerFuerza(Point3d mipos) throws IOException{
		cargarConstantes();
		
		double direccion = calcularDireccion(mipos);
		
		double magnitud = getMagnitudFuerza(mipos, predictor.predecirPosicion(idObjeto));
		
		//System.out.println("REPULSOR tangencial " + idObjeto + ": direccion = " + direccion + " magnitud = " + magnitud);
		 
		Vector resultante = new Vector(magnitud, direccion);  
		
		resultante.setMagnitud(Math.abs(magnitud));
		
		return resultante;
	};
}

